#ifndef IMUINTEGRATION_H
#define IMUINTEGRATION_H
#include <iostream>
#include <vector>

#include "math_utils.h"
#include "state.h"

class PoseObserve {
public:  
   static void ComputeResidualAndJacobian(const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 5, 1> &residual, Eigen::Matrix<double, 5, 8> &jacobian);               
   static void ComputeResidualAndJacobian(const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 2, 1> &residual, Eigen::Matrix<double, 2, 8> &jacobian);               
   static void ComputeResidualAndJacobianPoseGraph(const State& last_state, const Eigen::Vector3d &last_pose, const Eigen::Vector2d &last_velocity, const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 5, 1> &residualXi, Eigen::Matrix<double, 5, 8> &jacobianXi, Eigen::Matrix<double, 5, 1> &residualXj, Eigen::Matrix<double, 5, 5> &jacobianXj, Eigen::Matrix<double, 5, 5> &jacobianXij, Eigen::Matrix<double, 5, 8> &jacobianXji);               
   static void ComputeResidualAndNumericalJacobian(const State& last_state, const Eigen::Vector3d &last_pose, const Eigen::Vector2d &last_velocity, const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 5, 1> &residualXi, Eigen::Matrix<double, 5, 8> &jacobianXi, Eigen::Matrix<double, 5, 1> &residualXj, Eigen::Matrix<double, 5, 5> &jacobianXj, Eigen::Matrix<double, 5, 5> &jacobianXij, Eigen::Matrix<double, 5, 8> &jacobianXji);
private:
   static void ComputeResidual(const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 5, 1> &residual);
};

#endif 
